#include "zbStateMachine.h"


u8 nothing =0;
pktErr packetErr;
u16 bytesAvail = 0;
u16 respSize = 0;
u8 sleeping = false;
u8 dutyStop[8] = {0,0,0,0,0,0,0,0};

/**
  * @brief  main loop, calls functions when needed and processes
  * 		packets until there is enough data to find packet
  * @param  None
  * @retval None
  */
void stateMachineLoop(void)
{
		// Switch on the state of the robot
		switch(mygVars->dataState)
		{
			case idle: // 

				// restart int in case we overflowed our buffer
				USART_ITConfig(zbCom_USART, USART_IT_RXNE, ENABLE);

				// sleep and wake on RX of uart ? 
				mygVars->dataState = waitingfordata;

				// start idle timeout, if we stay in idle too long 
				// disable everything 
				if(idleTimeoutAt == 0)
				{
					idleTimeoutAt = (getSecondTick() + 1);
				} 

				break;

			case waitingfordata: 
				// bytes available in circular buffer
				bytesAvail = cbCount(&rxUSCBuff);

				if(bytesAvail == 0)
				{
					// if we have timed out shut down, low power
					if(sleeping == false && getSecondTick() > idleTimeoutAt)
					{
						idleTimeoutAt = 0;
						sleeping = true;
						// stop all motors, 0% power
						setupPWMOUTDuties(dutyStop, 0x0F);
						// sleeping
						printf("\n\rSleeping");
					}
				}

				// waiting for more data from a packet, should we timeout ? 
				if(packetErr.ERR == 1 && getSecondTick() > pktTimeoutAt)
				{
					pktTimeoutAt = 0;
					packetErr.ERR = 0;
					packetErr.SIZE = 0;
					// flush the rest of the buffer
					while(!cbIsEmpty(&rxUSCBuff))
					{
						cbRead(&rxUSCBuff, &nothing);
					}
					printf("%lu : Timeout", getSecondTick());
					mygVars->dataState = idle;
					break;
				}

				// if we have enough to make a packet and we are not waiting for more from a packet
				if(bytesAvail >= (hdrSize + crcSize + 4) || ( packetErr.ERR == 1 && bytesAvail >= packetErr.SIZE ) )
				{
					//printf("got data, checking");
					checkPacket(&rxUSCBuff, &packetErr);

					switch(packetErr.ERR)
					{
						case 0: // no error, valid packet
							mygVars->dataState = gotnewdata;
							pktTimeoutAt = 0;
							break;

						case 1: // we need more data 
							if(pktTimeoutAt == 0)
							{
								pktTimeoutAt = (getSecondTick() + 1);
							} 
							break;

						case 2: // crc error or invalid packet
							printf("%lu : CRC Err ", getSecondTick());
							mygVars->dataState = idle;
							break;
						
						case 3: // timeout waiting for write data
							printf("%lu : Timeout ", getSecondTick());
							mygVars->dataState = idle;
							break;

						case 4:
							printf("%lu : Invalid header ", getSecondTick());
							mygVars->dataState = idle;
							break;

						case 0xFF: // no valid data in packet !
							printf("%lu : No valid data in packet", getSecondTick());
							//cbInit(&rxUSCBuff); // reset circular buffer
							mygVars->dataState = idle;
							break;

						default:
							printf("%lu : Unknown data state", getSecondTick());
							mygVars->dataState = idle;
							break;

					}
					//RxCounter = 0;
					//dataState = gotnewdata;
				}

				// otherwise do something else here
				break;
			
			case gotnewdata:
				// Process the packet, all of the needed vars
				// were saved when we checked the packet
				respSize = processPacket();

				// reset our idle timeout, we are NOT idle !
				idleTimeoutAt = 0;
				// if we were asleep wake up ! 
				if(sleeping == true)
				{
					// we let the user turn stuff on, we automatically turn it off
					//newgVars->enabled = 0xFF; 	// turn everything on
					sleeping = false;
				}

				// make sure everything is enabled
				newgVars->enabled |= 0x17;

				// do everything that should be done
				doTasks();	  				// make it so

				// if we get a size back there is data ready to be
				// sent out, else there is nothing to respond 
				if(respSize)
				{
					mygVars->dataState = readytosenddata;
				}else{ 

					// if we dont get a response size we must have
					// gotten a write message, this is when we update
					// all of the items
					mygVars->dataState = idle;
				}
				break;

			case readytosenddata:
				// send out the data, todo get right size!
				DMA_TXPush();
				//BASIC_TXPush();
				mygVars->dataState = idle;
				break;

			case sendingdata:
				// more free time here
				break;

			case dataerror:
				// there is an error, just start over
				printf("Error Uart Flags: 0x%X", USART2->SR);
				mygVars->dataState = idle;
				break;

			default:
				printf("PacketState: %d\n\r", mygVars->dataState);
				break;
				//Delay(100);
		}
}
